package org.team751.sensors;

import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.SensorBase;
import edu.wpi.first.wpilibj.parsing.ISensor;

import org.team751.util.WPILibTimer;//WPILib stopwatch-type timer
import java.util.Timer;//Java thread-related timer
import java.util.TimerTask;
import org.team751.dev.RobotMap;

/**
 * Interfaces with a Parallax Ping))) ultrasonic rangefinder
 * @author Sam Crow
 */
public class ParallaxPing extends SensorBase implements ISensor {
    private DigitalModule module;
    private int channel;

    private PingTask task;

    Timer timer;

    /**
     * Constructor using the default digital module and a specified digital I/O channel
     * @param channel The I/O channel on the Digital Sidecar to use
     */
    public ParallaxPing(int channel){
        this.channel = channel;
        module = DigitalModule.getInstance(RobotMap.mainSlot);
        setupTask();
    }
    /**
     * Constructor using a specified digital module and a specified digital I/O channel
     * @param slot The cRIO module slot for the digital module
     * @param channel The I/O channel on the Digital Sidecar to use
     */
    public ParallaxPing(int slot, int channel){
        this.channel = channel;
        module = DigitalModule.getInstance(slot);
        setupTask();
    }
    /**
     * This function is called by every constructor to set up the PingTask that this class uses
     */
    private void setupTask(){
        task = new PingTask(module, channel);
    }

    /**
     * Start sending pulses and reading values
     */
    public void startPulses(){
        if(timer == null){
            timer = new Timer();
        }
        System.out.println(toString()+": About to schedule timer");
        timer.schedule(task, 0, 100);//Set the timer to execute the task every x miliseconds
    }

    public void stopPulses(){
        timer.cancel();
    }

    private class PingTask extends TimerTask {
        private DigitalModule module;
        private int channel;

        public PingTask(DigitalModule module, int channel){
            this.module = module;
            this.channel = channel;
        }

        /**
         * Run: Gets a reading from the sensor
         */
        public void run() {
            System.out.println(toString()+": Running");
            //start trigger pulse
            module.setDIO(channel, true);
            try {
                Thread.sleep(5);//wait 5 miliseconds for the sensor to register the pulse
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
            module.setDIO(channel, false);//set the channel low to end the pulse
            System.out.println(toString()+": Waiting for sensor to start return pulse at timestamp "+WPILibTimer.getFPGATimestamp());
            while(!module.getDIO(channel));//wait while the sensor has not set the line to high
            //Now the sensor has set the line to high.
            double returnPulseBeginTimestamp = WPILibTimer.getFPGATimestamp();//record the time that the return pulse started
            System.out.println(toString()+": Waiting for sensor to end return pulse at timestamp "+WPILibTimer.getFPGATimestamp());
            while(module.getDIO(channel));//wait while the sensor continues the high pulse
            //Now the sensor has set the line back to low and ended the pulse
            double returnPulseEndTimestamp = WPILibTimer.getFPGATimestamp();//record the time

            //calculate the time of the return pulse in seconds
            double pulseLength = returnPulseEndTimestamp - returnPulseBeginTimestamp;
            System.out.println("Pulse time: "+pulseLength+" seconds");
        }

    }
}
